#pragma config(UART_Usage, UART2, uartNotUsed, baudRate4800, IOPins, None, None)
#pragma config(Sensor, dgtl1,  upperLimit,     sensorTouch)
#pragma config(Sensor, dgtl7,  Indicator,      sensorDigitalOut)
#pragma config(Sensor, dgtl11, leftRight,      sensorTouch)
#pragma config(Sensor, dgtl12, strat,          sensorTouch)
#pragma config(Motor,  port2,           flipper,       tmotorVex393, openLoop)
#pragma config(Motor,  port3,           backLeft,      tmotorVex393, openLoop, reversed)
#pragma config(Motor,  port4,           conveyer,      tmotorVex393, openLoop)
#pragma config(Motor,  port5,           backRight,     tmotorVex393, openLoop)
#pragma config(Motor,  port6,           raiseFL,       tmotorVex393, openLoop)
#pragma config(Motor,  port7,           raiseBL,       tmotorVex393, openLoop, reversed)
#pragma config(Motor,  port8,           raiseBR,       tmotorVex393, openLoop)
#pragma config(Motor,  port9,           raiseFR,       tmotorVex393, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#define abs(X) ((X < 0) ? -1 * X : X)
#pragma platform(VEX)
#pragma competitionControl(Competition)
#pragma autonomousDuration(20)
#pragma userControlDuration(120)
#include "Vex_Competition_Includes.c"

task tankDriveUser();
void autonSpin(float rad);
void conveyerControlUser();
task userRaiserControl();
void autonDrive(int power, float distance);
void spinnerUserControl();
void raisePlatform(int power);
void lowerPlatform(int power);
float SPEED=0.05;
float RADIUS=7.5;
void pre_auton()
{
}
task autonomous()
{
	if(SensorValue(strat)==0)	{
		allMotorsOff();
		autonDrive(127,12);   //go forward 12"
		motor[conveyer]=127;  //dump the preload
		wait10Msec(300);
		allMotorsOff();
		if(SensorValue(leftRight)==1)	{
			autonSpin(-PI/2);//turn pi/2 rad
			} else {
			autonSpin(PI/2);
		}
		autonDrive(127,15);
		if(SensorValue(leftRight)==1) {
			autonSpin(PI/2);
			} else {
			autonSpin(-PI/2);
		}
		int power =127;
		motor[conveyer]=power;
		motor[flipper]=power;
		autonDrive(80,12);
		power =0;
		motor[conveyer]=power;
		motor[flipper]=power;
		autonSpin(PI);  //spin around 
		autonDrive(6,-127); //back it up
		raisePlatform(60);
		autonDrive(6,-127);
		motor[conveyer]=127;  //dump the load
		wait10Msec(500);
		motor[conveyer]=0;
		lowerPlatform(-100);
		} else {
	}
}

task usercontrol()
{
	StartTask(tankDriveUser);
	StartTask(userRaiserControl);
	while (true)
	{
		conveyerControlUser();
		spinnerUserControl();
		wait1Msec(1);
	}
}
task tankDriveUser() {
	while(true) {
		int left=0, right=0;
		int threshold = 5;
		if(abs(vexRT[Ch3])>=threshold)
			left =(vexRT[Ch3])*0.787;
		else
			left=0;
		if(abs(vexRT[Ch2])>=threshold)
			right = vexRT[Ch2]*0.787;
		else
			right=0;
		motor[backLeft]=left;
		motor[backRight]=right;
		wait1Msec(3);
	}
}
void conveyerControlUser()	{
	int power=0;
	if(vexRT[Btn6U]==1)	{
		power=127;
		} else if(vexRT[Btn6D]==1)	{
		power=-127;
		} else {
		power=0;
	}
	motor[conveyer]=power;
}
task userRaiserControl()	{
	int power=0;
	bool value2 = true;
	while(1==1)	{
		if(vexRT[Btn5U]==1)	{
			power=60;
			value2 = false;
			} else if(vexRT[Btn5D]==1) {
			power = -100;
			value2 = false;
			} else	{
			power= 0;
		}
		if(vexRT[Btn7U]==1)	{
			motor[raiseBL]=0;
			motor[raiseFL]=0;
			motor[raiseBR]=0;
			motor[raiseFR]=0;
			value2 = true;
			SensorValue(Indicator)=0;
		}
		if(!value2)	{
			motor[raiseBL]=power;
			motor[raiseFL]=power+17;
			motor[raiseBR]=power+10;
			motor[raiseFR]=power+20;
			SensorValue(Indicator)=1;
		}
		wait1Msec(3);
	}
}
void spinnerUserControl()	{
	int power;
	if(vexRT[Btn6U]==1)	{
		power=127;
		} else if(vexRT[Btn6D]==1)	{
		power=-127;
		} else 	{
		power=0;
	}
	motor[flipper]=power;
}
void autonDrive(int power, float distance)	{
	int waitTime= (int)(SPEED*distance*1000);
	motor[backLeft]=power;
	motor[backRight]=power;
	wait1Msec(waitTime);
	motor[backLeft]=0;
	motor[backRight]=0;
}

void autonSpin(float rad)	{
	int waitTime =(int)( SPEED*RADIUS*rad*1000);
	int left=0;
	int right=0;
	if(rad <0)	{
		left=-127;
		right=127;
		} else {
		left=127;
		right=-127;
	}
	motor[backLeft]=left;
	motor[backRight]=right;
	wait1Msec(waitTime);
	allMotorsOff();
}
void raisePlatform(int power) {
	bool hi = true;
	int startTime=time10[T1];
	while(hi) {
		motor[raiseBL]=power;
		motor[raiseFL]=power+17;
		motor[raiseBR]=power+10;
		motor[raiseFR]=power+20;
		if(SensorValue(upperLimit)==1||(time10[T1]-startTime)>=200) {
			power=0;
			hi=false;
		}
	}
}
void lowerPlatform(int power) {
	int startTime=time10[T1];
	while(time10[T1]-startTime>=200) {
		motor[raiseBL]=power;
		motor[raiseFL]=power+17;
		motor[raiseBR]=power+10;
		motor[raiseFR]=power+20;
	}
	motor[raiseBL]=0;
	motor[raiseFL]=0;
	motor[raiseBR]=0;
	motor[raiseFR]=0;
}